﻿using System;
using System.Collections.Generic;
using Kinect.Recognition.Adapters;
using Kinect.Recognition.States;
using Kinect.Recognition.Tracking;
using Microsoft.Research.Kinect.Nui;

using Matrix = Microsoft.Xna.Framework.Matrix;
using Vector3 = Microsoft.Xna.Framework.Vector3;
using Kinect;
using Kinect.GPart;

namespace Kinect
{
    public partial class KinectController
    {
        public Runtime nui;
        public double A, B, C, D;
        public double AA, BB, CC, DD;
        public Vector plane3d;

        public byte[] convertedDepthFrame;

        RuntimeOptions runtimeOptions { get; set; }
        PlanarImage Image;

        public SkeletonViewer ViewSkeleton;//骨架绘制控件
        public DepthViewer ViewDepth;//深度图数据

        public delegate void outGetHandPositionEventHandler(object sender, MouseEvent args);
        public outGetHandPositionEventHandler outHand;
        public delegate void zoomEventHandler(object sender);
        public zoomEventHandler outzoom;

        const int RED_IDX = 2;
        const int GREEN_IDX = 1;
        const int BLUE_IDX = 0;
        byte[] depthFrame32 = new byte[320 * 240 * 4];

        int[] Depth, player;
        int[] flag;
        public int imgHeight;
        public int imgWidth;
        int gap = 100;

        public int LeftX = -1, LeftY = -1, RightX = -1, RightY = -1;
        public int LeftWristX = -1, LeftWristY = -1, RightWristX = -1, RightWristY = -1;
        public int LeftTipX, LeftTipY, RightTipX, RightTipY;

        private const int msgCount = 13;
        private GesturesFSM<TrackingContext> fsm;
        public GesturesFSM<TrackingContext> FSM { get { return fsm; } }
        private TrackingContext context;
        public TrackingContext Info { get { return context; } }
        private StateRecording recordingState;
        private GFileReader gFileReader= new GFileReader();
        private Vector3[] poly3 = new Vector3[4];
        public String recordGesId;

        public Vector3 absolute2relative(Vector3 absolutePoint)
        {
            Matrix T = new Matrix();
            //Vector3 O = poly3[0];
            //Vector3 i = Vector3.Subtract(poly3[1], O);
            //Vector3 j = Vector3.Subtract(poly3[3], O);
            //Vector3 k = Vector3.Cross(i, j);
            Vector3 O = poly3[2];
            Vector3 i = Vector3.Subtract(poly3[3], O);
            Vector3 j = Vector3.Subtract(poly3[1], O);
            Vector3 k = Vector3.Cross(i, j);
            T.M11 = i.X; T.M12 = j.X; T.M13 = k.X;
            T.M21 = i.Y; T.M22 = j.Y; T.M23 = k.Y;
            T.M31 = i.Z; T.M32 = j.Z; T.M33 = k.Z;
            T.M44 = 1;
            T = Matrix.Invert(T);
            Vector3 v = Vector3.Subtract(absolutePoint, O);
            v = Vector3.Transform(v, T);
            //BEG
            v.X = handXfilter.Apply(v.X);
            v.Y = handYfilter.Apply(v.Y);
            //if (v.X < 0 || v.X > 1 || v.Y < 0 || v.Y > 1)
            //{
            //    v.X = v.Y = -1;
            //}
            //END
            v.Z = G3D.dist2plane(plane3d, absolutePoint);
            return v;
        }

        Filtering.LowPassDynamicFilter handXfilter, handYfilter; //TAT

        public KinectController(bool test)
        {
            //BEG
            handXfilter = new Filtering.LowPassDynamicFilter(10);
            handXfilter.SetCutoffFrequencyLow(3);
            handXfilter.SetCutoffFrequencyHigh(20);
            handXfilter.SetVelocityLow(50);
            handXfilter.SetVelocityHigh(100);

            handYfilter = new Filtering.LowPassDynamicFilter(10);
            handYfilter.SetCutoffFrequencyLow(3);
            handYfilter.SetCutoffFrequencyHigh(20);
            handYfilter.SetVelocityLow(50);
            handYfilter.SetVelocityHigh(100);
            //END++
            gFileReader.open();
            A = gFileReader.readlnDouble();
            B = gFileReader.readlnDouble();
            C = -1;
            D = gFileReader.readlnDouble();

            plane3d = new Vector();
            plane3d.X = (float)A;
            plane3d.Y = (float)B;
            plane3d.Z = (float)C;
            plane3d.W = (float)D;

            for (int i = 0; i < 4; i++)
            {
                poly3[i] = new Vector3();
                gFileReader.readln(ref poly3[i]);
                screenz += poly3[i].Z;
            }
            screenz /= 4;
            AA = gFileReader.readlnDouble();
            BB = gFileReader.readlnDouble();
            CC = -1;
            DD = gFileReader.readlnDouble();

            gFileReader.close();

            //Get the template position of player
                GFileReader PReader = new GFileReader();
                PReader.open("e:/GGBadguy.txt");
                    templatePosition = PReader.test_readln();
                PReader.close();
            //OK！

            try
            {
                nui = Runtime.Kinects[0];
            }
            catch (ArgumentOutOfRangeException)
            {
                System.Windows.MessageBox.Show("Ensure a Kinect device is plugged in.");
                System.Environment.Exit(System.Environment.ExitCode);
            }
            try
            {
                runtimeOptions = RuntimeOptions.UseDepthAndPlayerIndex | RuntimeOptions.UseSkeletalTracking;
                nui.Initialize(runtimeOptions);
                this.InitFSM(test);
            }
            catch (InvalidOperationException)
            {
                System.Windows.MessageBox.Show("Runtime initialization failed. Please make sure Kinect device is plugged in.");
                return;
            }

            try
            {
                //nui.VideoStream.Open(ImageStreamType.Video, 2, ImageResolution.Resolution640x480, ImageType.Color);
                nui.DepthStream.Open(ImageStreamType.Depth, 2, ImageResolution.Resolution320x240, ImageType.DepthAndPlayerIndex);
                if (runtimeOptions.HasFlag(RuntimeOptions.UseSkeletalTracking))
                    nui.SkeletonEngine.TransformSmooth = true;
            }
            catch (InvalidOperationException)
            {
                System.Windows.MessageBox.Show("Failed to open stream. Please make sure to specify a supported image type and resolution.");
                return;
            }

            ViewSkeleton = new SkeletonViewer();
            ViewDepth = new DepthViewer();
            ViewDepth.MyRuntimeOptions = runtimeOptions;
            ViewDepth.PlaneAlready += new DepthViewer.AlreadyGetPlaneEventHandler(nui_planeAlready);

            nui.DepthFrameReady += new EventHandler<ImageFrameReadyEventArgs>(nui_DepthFrameReady);
            nui.SkeletonFrameReady += new EventHandler<SkeletonFrameReadyEventArgs>(nui_SkeletonFrameReady);
            InitSpeechRecognizer();
        }

        double screenz ;
        double handgap = 0.5;
        public float Handx = -1;
        public float Handy = -1;
        public float Gx, Gy, Gz;
        public GPoint GHand = new GPoint();
        float Handz = -1;
        Vector3? Hand = new Vector3();
        public bool depthFlag = false;


        void nui_planeAlready(object sender)
        {
            gFileReader.open();
            A = gFileReader.readlnDouble();
            B = gFileReader.readlnDouble();
            C = -1;
            D = gFileReader.readlnDouble();
            for (int i = 0; i < 4; i++)
            {
                poly3[i] = new Vector3();
                gFileReader.readln(ref poly3[i]);
                screenz+=poly3[i].Z;
            }
            screenz/=4;
            AA = gFileReader.readlnDouble();
            BB = gFileReader.readlnDouble();
            CC = -1;
            DD = gFileReader.readlnDouble();
            gFileReader.close();
        }

        void nui_DepthFrameReady(object sender, ImageFrameReadyEventArgs e)
        {
            Image = e.ImageFrame.Image;
            convertedDepthFrame = convertDepthFrame(Image.Bits, ref Depth, ref player);

            // <start insert>
            imgHeight = Image.Height;
            imgWidth = Image.Width;

            flag = new int[320 * 240 * 4];

            //splitDepthFrame(Image.Bits, out Depth, out player);

            gao(LeftX, LeftY, LeftWristX, LeftWristY, out LeftTipX, out LeftTipY);

            GFuck.FuckBorder(convertedDepthFrame, imgWidth, imgHeight, 10, LeftTipX, LeftTipY);

            if (fsm.Current != null)
                fsm.Current.ProcessDepth(e.ImageFrame);
            ViewDepth._Kinect_DepthFrameReady(sender, e);
        }

        /// <summary>
        /// 静态动作判断
        /// </summary>
        double[] lefthand = new double[3];
        double[] righthand = new double[3];
        double distanceofhand = 0;
        public bool zoomflag = true;
        void distance()
        {
            for (int i = 0; i < 3; i++)
                distanceofhand += Math.Pow(lefthand[i] - righthand[i], 2);
            if (distanceofhand > 1.3)
                now = DateTime.Now.Ticks;
            else
                lasttime = DateTime.Now.Ticks;
            if (now - lasttime > (new TimeSpan(0, 0, 1).Ticks)/2)
            {
                if(outzoom!=null)
                    outzoom(this);
                //lasttime = DateTime.Now.Ticks + new TimeSpan(0,0,10).Ticks;
                zoomflag = false;
            }
                distanceofhand = 0;
        }
        long lasttime=DateTime.Now.Ticks;
        long now = DateTime.Now.Ticks;
        //静态动作判断完毕
        void nui_SkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
        {
            SkeletonFrame skeletonFrame = e.SkeletonFrame;
            int iSkeleton = 0;
            if (skeletonFrame == null)
                return;

            // <start insert>
            foreach (SkeletonData data2 in skeletonFrame.Skeletons)
            {

                if (data2.TrackingState == SkeletonTrackingState.Tracked)
                {
                    //zoomBegin
                    if (zoomflag)
                    {
                        righthand[0] = data2.Joints[JointID.HandRight].Position.X;
                        righthand[1] = data2.Joints[JointID.HandRight].Position.Y;
                        righthand[2] = data2.Joints[JointID.HandRight].Position.Z;

                        lefthand[0] = data2.Joints[JointID.HandLeft].Position.X;
                        lefthand[1] = data2.Joints[JointID.HandLeft].Position.Y;
                        lefthand[2] = data2.Joints[JointID.HandLeft].Position.Z;

                        if(Math.Abs(righthand[1]-lefthand[1])<0.5)
                        distance();
                    }
                    if (GetPlayerPostion)
                    {
                        setPlayerPosition(data2.Joints[JointID.Spine]);
                        CheckIfInThePosition();
                    }
                    //zoomEnd

                    //gethandBegin
                    Handz = data2.Joints[JointID.HandLeft].Position.Z;
                    if (Handz + handgap - screenz > 0)
                        depthFlag = true;
                    else
                        depthFlag = false;
                    if (depthFlag)
                    {
                        Handx = data2.Joints[JointID.HandLeft].Position.X;
                        Handy = data2.Joints[JointID.HandLeft].Position.Y;
                        Hand = absolute2relative(new Vector3(Handx, Handy, Handz));
                        Handx = Hand.Value.X;
                        Handy = Hand.Value.Y;
                        GHand = new GPoint(Handx, Handy);
                        //Gx = data2.Joints[JointID.HandLeft].Position.X;
                        //Gy = data2.Joints[JointID.HandLeft].Position.Y;
                        //Gz = data2.Joints[JointID.HandLeft].Position.Z;
                        //MouseEvent args = new MouseEvent(Hand.Value.X * 1366, Hand.Value.Y * 768);
                        //this.outHand(this, args);
                    }
                    //gethandEnd
                    getPosition(data2.Joints[JointID.HandLeft], out LeftX, out LeftY);
                    getPosition(data2.Joints[JointID.HandRight], out RightX, out RightY);
                    getPosition(data2.Joints[JointID.WristLeft], out LeftWristX, out LeftWristY);
                    getPosition(data2.Joints[JointID.WristRight], out RightWristX, out RightWristY);

                    //float x = data.Joints[JointID.HandRight].Position.X;
                    //float y = data.Joints[JointID.HandRight].Position.Y;
                    //float z = data.Joints[JointID.HandRight].Position.Z;
                    //float w = data.Joints[JointID.HandRight].Position.W;

                    //GTest.Text = x + "\t" + y;
                    //GTest2.Text = z + "\t" + w;
                } 
                iSkeleton++;
            }
            if (fsm.Current != null)
                fsm.Current.ProcessSkeletons(new SkeletonFrameAdapter(e.SkeletonFrame));

            //绘制骨架显示控件,顺序固定
            ViewSkeleton.cuejoint = Info.CurrentCue;
            ViewSkeleton.DrawSkeleton(skeletonFrame);
        }
        byte[] convertDepthFrame(byte[] depthFrame16, ref int []depthFrame, ref int []playerFrame)
        {
            if (depthFrame == null) depthFrame = new int[320 * 240 * 4];
            if (playerFrame == null) playerFrame = new int[320 * 240 * 4];

            for (int i16 = 0, i32 = 0, i8 = 0; i16 < depthFrame16.Length && i32 < depthFrame32.Length; i16 += 2, i32 += 4, i8++)
            {
                int player = depthFrame16[i16] & 0x07;
                int realDepth = (depthFrame16[i16 + 1] << 5) | (depthFrame16[i16] >> 3);
                // transform 13-bit depth information into an 8-bit intensity appropriate
                // for display (we disregard information in most significant bit)
                byte intensity = (byte)(255 - (255 * realDepth / 0x0fff));

                depthFrame[i8] = realDepth;
                playerFrame[i8] = player;

                depthFrame32[i32 + RED_IDX] = 100;
                depthFrame32[i32 + GREEN_IDX] = 100;
                depthFrame32[i32 + BLUE_IDX] = 0;

                // choose different display colors based on player
                switch (player)
                {
                    case 0:
                        depthFrame32[i32 + RED_IDX] = 0;
                        depthFrame32[i32 + GREEN_IDX] = 0;
                        depthFrame32[i32 + BLUE_IDX] = 0;
                        break;
                    case 1:
                        depthFrame32[i32 + RED_IDX] = intensity;
                        break;
                    case 2:
                        depthFrame32[i32 + GREEN_IDX] = intensity;
                        break;
                    case 3:
                        depthFrame32[i32 + RED_IDX] = (byte)(intensity / 4);
                        depthFrame32[i32 + GREEN_IDX] = (byte)(intensity);
                        depthFrame32[i32 + BLUE_IDX] = (byte)(intensity);
                        break;
                    case 4:
                        depthFrame32[i32 + RED_IDX] = (byte)(intensity);
                        depthFrame32[i32 + GREEN_IDX] = (byte)(intensity);
                        depthFrame32[i32 + BLUE_IDX] = (byte)(intensity / 4);
                        break;
                    case 5:
                        depthFrame32[i32 + RED_IDX] = (byte)(intensity);
                        depthFrame32[i32 + GREEN_IDX] = (byte)(intensity / 4);
                        depthFrame32[i32 + BLUE_IDX] = (byte)(intensity);
                        break;
                    case 6:
                        depthFrame32[i32 + RED_IDX] = (byte)(intensity / 2);
                        depthFrame32[i32 + GREEN_IDX] = (byte)(intensity / 2);
                        depthFrame32[i32 + BLUE_IDX] = (byte)(intensity);
                        break;
                    case 7:
                        depthFrame32[i32 + RED_IDX] = (byte)(255 - intensity);
                        depthFrame32[i32 + GREEN_IDX] = (byte)(255 - intensity);
                        depthFrame32[i32 + BLUE_IDX] = (byte)(255 - intensity);
                        break;
                }
            }

            return depthFrame32;
        }

        //void splitDepthFrame(byte[] depthFrame16, out int[] Depth, out int[] player)
        //{
        //    Depth = new int[320 * 240 * 4];
        //    player = new int[320 * 240 * 4];

        //    bool hasPlayerData = runtimeOptions.HasFlag(RuntimeOptions.UseDepthAndPlayerIndex);
        //    for (int i16 = 0, j = 0; i16 < depthFrame16.Length; i16 += 2, j++)
        //    {
        //        player[j] = hasPlayerData ? depthFrame16[i16] & 0x07 : -1;
        //        int realDepth = 0;

        //        if (hasPlayerData)
        //            realDepth = (depthFrame16[i16 + 1] << 5) | (depthFrame16[i16] >> 3);
        //        else
        //            realDepth = (depthFrame16[i16 + 1] << 8) | (depthFrame16[i16]);
        //        Depth[j] = realDepth;
        //    }
        //}

        void clearFlag()
        {
            for (int i = 0; i < imgHeight; i++)
            {
                for (int j = 0; j < imgWidth; j++)
                {
                    flag[i * imgWidth + j] = 0;
                }
            }
        }

        int []gaoQueueX = new int[GParam.depthWidth * GParam.depthHeight];
        int []gaoQueueY = new int[GParam.depthWidth * GParam.depthHeight];
        
        void gao2(int x1, int y1, int x2, int y2, out int tipX, out int tipY)
        {
            tipX = tipY = -1;
            if (x1 < 0 || x1 >= imgWidth) return;
            if (x2 < 0 || x2 >= imgWidth) return;
            if (y1 < 0 || y1 >= imgHeight) return;
            if (y2 < 0 || y2 >= imgHeight) return;

            int area = 0;

            //Pair<int, int> p1 = new Pair<int, int>(x1, y1);
            //Pair<int, int> p2 = new Pair<int, int>(x2, y2);

            GUtil.swap(ref x1, ref y1);
            GUtil.swap(ref x2, ref y2);

            //x1 = p1.Second;
            //y1 = p1.First;
            //x2 = p2.Second;
            //y2 = p2.First;

            clearFlag();

            //bfs x1, y1
            int[] dx = { -1, 0, 1, 0 };
            int[] dy = { 0, 1, 0, -1 };

            int gaoQueueBase = 0, gaoQueueTop = 0;
            int maxarea = imgWidth * imgHeight / 50;
//            Queue<Pair<int, int>> Q = new Queue<Pair<int, int>>();
            gaoQueueX[gaoQueueTop] = x1;
            gaoQueueY[gaoQueueTop++] = y1; 
            //Q.Enqueue(new Pair<int, int>(x1, y1));
            flag[x1 * imgWidth + y1] = 1;


            while (gaoQueueBase != gaoQueueTop)
            {
                int x = gaoQueueX[gaoQueueBase];
                int y = gaoQueueY[gaoQueueBase++];
                //Pair<int, int> pt = Q.Dequeue();
                //int x = pt.First;
                //int y = pt.Second;
                if ((x1 - x2) * (x - x2) + (y1 - y2) * (y - y2) < 0) continue;
                for (int k = 0; k < 4; k++)
                {
                    int nx = x + dx[k];
                    int ny = y + dy[k];
                    if (nx < 0 || nx >= imgHeight) continue;
                    if (ny < 0 || ny >= imgWidth) continue;
                    if (flag[nx * imgWidth + ny] != 0) continue;
                    //                    if (player[nx * imgWidth + ny] == -1) continue;
                    if (Math.Abs(Depth[nx * imgWidth + ny] - Depth[x * imgWidth + y]) < gap)
                    {
                        gaoQueueX[gaoQueueTop] = nx;
                        gaoQueueY[gaoQueueTop++] = ny;
//                        Q.Enqueue(new Pair<int, int>(nx, ny));
                        flag[nx * imgWidth + ny] = 1;
                        if (++area > maxarea)
                        {
                            gaoQueueBase = gaoQueueTop = 0;
                            break;
                        }
                    }
                }
            }
            if (area > maxarea) clearFlag();

            int maxdist = -1;

            for (int i = 0; i < imgHeight; i++)
            {
                for (int j = 0; j < imgWidth; j++)
                {
                    if (flag[i * imgWidth + j] == 0) continue;
                    int thisdist = (x1 - x2) * (i - x2) + (y1 - y2) * (j - y2);
                    if (thisdist > maxdist)
                    {
                        maxdist = thisdist;
                        tipX = j;
                        tipY = i;
                    }
                }
            }        
        }

        void gao(int x1, int y1, int x2, int y2, out int tipX, out int tipY)
        {
            tipX = tipY = -1;
            if (x1 < 0 || x1 >= imgWidth) return;
            if (x2 < 0 || x2 >= imgWidth) return;
            if (y1 < 0 || y1 >= imgHeight) return;
            if (y2 < 0 || y2 >= imgHeight) return;

            int area = 0;

            Pair<int, int> p1 = new Pair<int, int>(x1, y1);
            Pair<int, int> p2 = new Pair<int, int>(x2, y2);

            x1 = p1.Second;
            y1 = p1.First;
            x2 = p2.Second;
            y2 = p2.First;

            clearFlag();

            //bfs x1, y1
            int[] dx = { -1, 0, 1, 0 };
            int[] dy = { 0, 1, 0, -1 };
            Queue<Pair<int, int>> Q = new Queue<Pair<int, int>>();
            Q.Enqueue(new Pair<int, int>(x1, y1));
            flag[x1 * imgWidth + y1] = 1;


            while (Q.Count > 0)
            {
                Pair<int, int> pt = Q.Dequeue();
                int x = pt.First;
                int y = pt.Second;
                if ((x1 - x2) * (x - x2) + (y1 - y2) * (y - y2) < 0) continue;
                for (int k = 0; k < 4; k++)
                {
                    int nx = x + dx[k];
                    int ny = y + dy[k];
                    if (nx < 0 || nx >= imgHeight) continue;
                    if (ny < 0 || ny >= imgWidth) continue;
                    if (flag[nx * imgWidth + ny] != 0) continue;
                    //                    if (player[nx * imgWidth + ny] == -1) continue;
                    if (Math.Abs(Depth[nx * imgWidth + ny] - Depth[x * imgWidth + y]) < gap)
                    {
                        Q.Enqueue(new Pair<int, int>(nx, ny));
                        flag[nx * imgWidth + ny] = 1;
                        area++;
                    }
                }
            }
            if (area > imgWidth * imgHeight / 50) clearFlag();

            int maxdist = -1;

            for (int i = 0; i < imgHeight; i++)
            {
                for (int j = 0; j < imgWidth; j++)
                {
                    if (flag[i * imgWidth + j] == 0) continue;
                    int thisdist = (x1 - x2) * (i - x2) + (y1 - y2) * (j - y2);
                    if (thisdist > maxdist)
                    {
                        maxdist = thisdist;
                        tipX = j;
                        tipY = i;
                    }
                }
            }
        }

        public class Pair<T, U>
        {
            public Pair()
            {
            }

            public Pair(T first, U second)
            {
                this.First = first;
                this.Second = second;
            }

            public T First { get; set; }
            public U Second { get; set; }

            public override bool Equals(object obj)
            {
                if (obj == null)
                    return false;
                if (obj == this)
                    return true;
                Pair<T, U> other = obj as Pair<T, U>;
                if (other == null)
                    return false;

                return
                    (((First == null) && (other.First == null))
                        || ((First != null) && First.Equals(other.First)))
                      &&
                    (((Second == null) && (other.Second == null))
                        || ((Second != null) && Second.Equals(other.Second)));
            }

        };

        private void getPosition(Joint joint, out int depthX, out int depthY)
        {
            float depthXf, depthYf;
            nui.SkeletonEngine.SkeletonToDepthImage(joint.Position, out depthXf, out depthYf);
            depthXf = depthXf * 320; //convert to 320, 240 space
            depthYf = depthYf * 240; //convert to 320, 240 space
            depthX = Convert.ToInt32(depthXf);
            depthY = Convert.ToInt32(depthYf);
        }

        ~KinectController()
        {
            if (nui != null)
                nui.Uninitialize();
            SpeechRecognizerExit();
        }

    }
}
